//
// Created by user on 25-9-9.
//

#include "camera.h"
#include <esp_camera.h>
#include <img_converters.h>
#include <cstring>

// 摄像头配置 - 适用于XIAO ESP32S3摄像头
camera_config_t Camera::camera_config = {
    .pin_pwdn = -1,
    .pin_reset = -1,
    .pin_xclk = 10,
    .pin_sscb_sda = 40,
    .pin_sscb_scl = 39,

    .pin_d7 = 48,
    .pin_d6 = 11,
    .pin_d5 = 12,
    .pin_d4 = 14,
    .pin_d3 = 16,
    .pin_d2 = 18,
    .pin_d1 = 17,
    .pin_d0 = 15,
    .pin_vsync = 38,
    .pin_href = 47,
    .pin_pclk = 13,

    .xclk_freq_hz = 20000000,
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_JPEG,
    .frame_size = FRAMESIZE_QVGA,
    .jpeg_quality = 12,
    .fb_count = 1,
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};

// Camera类构造函数
Camera::Camera() {
    // 构造函数中可以添加初始化代码
}

// 初始化摄像头
bool Camera::begin() {
    if (is_initialised) return true;

    // 初始化摄像头
    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK) {
        Serial.printf("摄像头初始化失败: 0x%x\n", err);
        return false;
    }

    is_initialised = true;
    return true;
}

// 释放摄像头资源
void Camera::end() {
    esp_err_t err = esp_camera_deinit();
    if (err != ESP_OK) {
        Serial.println("摄像头释放失败");
    }
    is_initialised = false;
}

// 捕获图像（直接返回帧缓冲区）
camera_fb_t* Camera::capture() {
    if (!is_initialised) {
        Serial.println("错误: 摄像头未初始化");
        return nullptr;
    }

    camera_fb_t *fb = esp_camera_fb_get();
    if (!fb) {
        Serial.println("摄像头捕获失败");
        return nullptr;
    }

    return fb;
}

// 捕获图像并转换为RGB888格式
bool Camera::capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
    if (!is_initialised) {
        Serial.println("错误: 摄像头未初始化");
        return false;
    }

    camera_fb_t *fb = esp_camera_fb_get();
    if (!fb) {
        Serial.println("摄像头捕获失败");
        return false;
    }

    // 只有当输入是JPEG格式时才进行转换
    if (fb->format == PIXFORMAT_JPEG) {
        bool converted = fmt2rgb888(fb->buf, fb->len, PIXFORMAT_JPEG, out_buf);
        esp_camera_fb_return(fb);
        
        if (!converted) {
            Serial.println("JPEG到RGB888转换失败");
            return false;
        }
    } else {
        // 如果已经是RGB格式，直接复制数据
        memcpy(out_buf, fb->buf, fb->len);
        esp_camera_fb_return(fb);
    }

    // 如果需要调整图像尺寸以匹配指定的宽高
    // 这里可以添加图像尺寸调整的代码
    // 为简化起见，我们假设输入尺寸与模型输入尺寸匹配

    return true;
}

// 捕获图像并通过串口发送
bool Camera::captureAndSend() {
    if (!is_initialised) {
        Serial.println("错误: 摄像头未初始化");
        return false;
    }

    camera_fb_t *fb = esp_camera_fb_get();
    if (!fb) {
        Serial.println("摄像头捕获失败");
        return false;
    }

    // 通过串口发送JPEG图像数据
    Serial.write(fb->buf, fb->len);
    
    // 打印图像信息（用于调试）
    Serial.printf("\n图像信息: 宽度=%d, 高度=%d, 大小=%d字节, 格式=%s\n", 
                  fb->width, fb->height, fb->len,
                  fb->format == PIXFORMAT_JPEG ? "JPEG" : 
                  fb->format == PIXFORMAT_RGB565 ? "RGB565" : 
                  fb->format == PIXFORMAT_RGB888 ? "RGB888" : 
                  fb->format == PIXFORMAT_GRAYSCALE ? "GRAYSCALE" : "UNKNOWN");
    
    esp_camera_fb_return(fb);
    return true;
}

// 设置图像格式
bool Camera::setPixelFormat(pixformat_t format) {
    if (!is_initialised) {
        Serial.println("错误: 摄像头未初始化");
        return false;
    }
    
    sensor_t *s = esp_camera_sensor_get();
    if (s == nullptr) {
        Serial.println("无法获取传感器实例");
        return false;
    }
    
    esp_err_t err = s->set_pixformat(s, format);
    if (err != ESP_OK) {
        Serial.printf("设置图像格式失败: 0x%x\n", err);
        return false;
    }
    
    // 更新配置
    camera_config.pixel_format = format;
    return true;
}

// 设置图像帧大小
bool Camera::setFrameSize(framesize_t size) {
    if (!is_initialised) {
        Serial.println("错误: 摄像头未初始化");
        return false;
    }
    
    sensor_t *s = esp_camera_sensor_get();
    if (s == nullptr) {
        Serial.println("无法获取传感器实例");
        return false;
    }
    
    esp_err_t err = s->set_framesize(s, size);
    if (err != ESP_OK) {
        Serial.printf("设置图像帧大小失败: 0x%x\n", err);
        return false;
    }
    
    // 更新配置
    camera_config.frame_size = size;
    return true;
}

// 检查摄像头是否已初始化
bool Camera::isInitialized() const {
    return is_initialised;
}

// 将JPEG图像转换为RGB888格式
bool Camera::jpegToRGB888(camera_fb_t* fb, uint8_t* output_buffer) {
    if (!is_initialised || fb == nullptr || output_buffer == nullptr) {
        Serial.println("错误: 参数无效");
        return false;
    }
    
    if (fb->format != PIXFORMAT_JPEG) {
        Serial.println("错误: 图像不是JPEG格式");
        return false;
    }
    
    // 使用ESP32的图像转换函数将JPEG转换为RGB888
    bool result = fmt2rgb888(fb->buf, fb->len, PIXFORMAT_JPEG, output_buffer);
    if (!result) {
        Serial.println("JPEG到RGB888转换失败");
        return false;
    }
    
    return true;
}

// 调整图像大小（简单的最近邻插值算法）
bool Camera::resizeImage(uint8_t* input_buffer, int input_width, int input_height, 
                         uint8_t* output_buffer, int output_width, int output_height) {
    if (input_buffer == nullptr || output_buffer == nullptr) {
        Serial.println("错误: 缓冲区指针无效");
        return false;
    }
    
    // 使用最近邻插值算法调整图像大小
    for (int y = 0; y < output_height; y++) {
        for (int x = 0; x < output_width; x++) {
            // 计算在原图中的对应位置
            int src_x = x * input_width / output_width;
            int src_y = y * input_height / output_height;
            
            // 边界检查
            src_x = src_x < input_width ? src_x : input_width - 1;
            src_y = src_y < input_height ? src_y : input_height - 1;
            
            // 对于RGB图像，每个像素占3个字节
            int src_index = (src_y * input_width + src_x) * 3;
            int dst_index = (y * output_width + x) * 3;
            
            // 复制RGB三个通道
            output_buffer[dst_index] = input_buffer[src_index];         // R
            output_buffer[dst_index + 1] = input_buffer[src_index + 1]; // G
            output_buffer[dst_index + 2] = input_buffer[src_index + 2]; // B
        }
    }
    
    return true;
}